/*   
 ============= 【MEA】 讹诈 攻击无人机 无人机机体程序 v1.2 ___ by MEA群主   
    
 【使用方法】   
 1、安装程序 —— 安装被程序到无人机编程块中，填写好需要填写的方块名称。   
 2、检测 —— 在无人机与母船脱离的情况下，使用Debug指令运行一次检测方块获取情况。第一行会显示母船控制座椅未找到，这是正常的，第二行显示“无人机方块初始化完成”即可。   
 3、将无人机安装到母船后，在代码中设置好无人机的进攻动作。   
 4、使用母船的中控计算机给无人机下达指令。   
    
【更新说明】
 v1.0
 1、修复了因为官方修改API造成的推进器控制问题
 2、增加了起飞点设定，方便机库设计
 
 v1.2
 更新了瞄准的PID控制，大大增加瞄准精度
 
 
 通讯协议：   
 本程序由中控计算机控制，并传输目标信息   
 标准指令格式：EZ@1指令@2目标座标@3目标速度@4目标半径 [飞船序号]   
 指令：   
 Attack @ 目标座标 @ 目标速度 @ 目标    
 Wing   
 Dock   
 Die   
 Sentry
*/   
   
// ======= 方块命名 =============   
//指定方块名字，留空程序会自动按类型获取   
string[] DetachBlockNameTag = {""}; //留空表示程序自动获取，可以是多个，多个请在{}内用,符号分割，并且必须加双引号。多个也可以留空，留空同样会自动获取   
int[] DetachBlockType = {1}; //分离块种类，数量必须和分离块的数量一致，0是合并块，1是连接器   
   
string MotherCockpitNameTag = "Cockpit"; //必填，指定母船的主控座椅名字，用于获取母船的飞行和指向状态   
string CockpitNameTag = ""; //远程控制块名字，留空自动获取
string GyroscopesNameTag = ""; //陀螺仪名字，留空自动获取
string ThrustsNameTag = ""; //推进器名字，留空自动获取
   
   
// ======= 作战设定 ============   
//这是3个互相匹配的参数，它们都是一列数组，并且必须保证它们的数量一致。   
//使用英文逗号来分隔，你需要几个动作就填写几个   
string[] ActionBlockNameTag = {"Gun_Timer", "Boom_Timer"}; //触发的方块名称，必须使用英文双引号   
string[] ActionName = {"TriggerNow", "TriggerNow"};//触发的方块动作，必须使用英文双引号   
double[] ActionDistanceToTarget = {1000, 100};//触发对应方块的距离（无人机与目标的距离），可以是小数
double BulletInitSpeed = 400; //武器子弹初速度   
double BulletAcceleration = 0; //武器子弹加速度   
double BulletMaxSpeed = 400; //武器子弹最大速度
double AttackScattering = 0; //子弹散射范围倍数，2是两倍目标直径圆，1是刚好散射到目标直径圆内，设为0是不散射。   
   
double CombatRadius = 900; //有效作战半径，无人机会尽量在这个半径内开启规避动作，在这个半径外执行接近动作，并最终趋近于保持在这个距离上与敌人作战   
   
// ====== 伴飞设定 ============   
Vector3D WingFlyPosition = new Vector3D(30,10,-35); //伴飞相对座标。在括号内填入座标，分别是X,Y,Z，其中X左-右+，Y上+下-，Z前-后+。这个座标是基于母船主控座椅为0点的   
double WingFlyDistanceMultiple = 5; // 自动伴飞距离倍数。如果设置，飞船会启用自动伴飞位置进行伴飞。自动伴飞位置是母船控制座椅到本无人机对应母船连接器的连线，乘以这个数值的位置。这个参数设为0飞船会使用上面的设定座标进行伴飞   
double SafeDistanceToMotherShip = 15; //防撞安全距离。与母船控制座椅的安全距离，这个距离应该设为比母船半径大20%左右。   
   
   
// ====== 自定义参数 ============   
// 指向控制部分   
double GyroscopesControlRatio = 0.8; // 陀螺仪控制力度倍数。1表示倍，必须大于0。作用于所有的指向控制部分   
   
// 起飞部分   
Vector3D LaunchPoint = new Vector3D(0,0,0); //起飞点，飞船每次从母船脱离都会先抵达起飞点再执行之后的动作。若设为0表示跳过起飞动作。   
   
// 螺旋运动部分 （攻击模式下生效）   
double Roll_R = 100; //圆周运动加速度“半径”，这个半径是加速度的半径，而不是真实的运动半径。它相当于在圆周运动过程中加速度的线速度大小，实际加速度效应相当于这个值的^0.5   
double Roll_Speed = 0.5; //加速度做圆周运动的大小   
   
// 停泊部分   
int DockSpeed = 20; //停泊速度，必须是正整数。数值越大停泊越慢。例如设为60，意味着每60个程序循环减少一次停泊距离。程序循环的速度是每秒60次。   
double DockDistance = 20; //停泊起始距离，无人机会在对应连接器前方这个距离处开始停泊。   
   
// 辅助飞行部分   
bool HelpMotherFly = false; //停泊后是否使用自己的推进器帮助母船飞行，是设为true，否设为false   
   
// 标识天线   
// 天线仅在非攻击模式会开启，且母船与无人机距离小于你设定的天线广播范围的时候，无人机的天线会被命名为[EZ #ID]，ID表示该无人机在控制面板的序号   
string AntennaCustomTag = ""; //天线标识。注意这不是天线名字，你需要把这个表示写入天线的CustomData中，如果填写，飞船会使用该天线。否则飞船会随机使用一个。天线仅用于标记飞船的名字   

// ================= 瞄准控制系统设置 ===============
// PID控制
double AimRatio = 5; //瞄准精度，单位：度。用来判断炮台是否瞄准，以便其他动作判断。不影响瞄准的效率。当瞄准块的正前方向量与瞄准块和目标的连线向量夹角小于这个值时，整个系统判定瞄准了目标。
int Aim_PID_T = 5; //PID 采样周期（单位：帧），周期越小效果越好，但太小的周期会让积分系数难以发挥效果
double AimPID_P = 0.8; //比例系数：可以理解为整个PID控制的总力度，建议范围0到1.2，1是完全出力。
double AimPID_I = 3; //积分系数：增加这个系数会让静态误差增加（即高速环绕误差），但会减少瞄准的震荡。反之同理
double AimPID_D = 20; //微分系数：增加这个系数会减少瞄准的震荡幅度，但会加剧在小角度偏差时的震荡幅度。反之同理
// ------ 自定义参数结束 --------   
   
   
// ========== 程序正文开始 ============   
int t;   
int init;   
string Debug;   
string MissileCommand;   
void Main(string arguments)   
{   
	if(arguments == "Debug"){   
		//GetMotherShip();   
		GetBlocks();   
		Echo(Debug);   
		MotionBlocksControl(false);   
	}   
	//开始指令   
	if(arguments.StartsWith("EZ@"))   
	{   
		//输出调试信息   
		Echo(Debug);   
		   
		//时钟   
		t ++;   
		   
		//执行指令变化的动作   
		if(arguments.Split('@')[1] != MissileCommand){   
			isLaunched = false;   
		}   
		   
		//获取指令   
		MissileCommand = arguments.Split('@')[1];   
   
		//初始化方块   
		if(init < 3)   
		{   
			DockStep = DockDistance; //初始化停泊距离   
			//获取到母船相关方块   
			if(init == 0 && GetMotherShip()){   
				init = 1;   
			}   
			//分离飞船   
			else if(init == 1 && MissileCommand != "Hold" && ShipDetach(true)){   
				init = 2;   
			}   
			else if(init == 2){   
				if(GetBlocks()){   
					init = 3;   
				}   
			}   
			return;   
		}   
		   
		//更新制导数据   
		NavigationInfoUpdata(true, arguments);   
		   
		//初始化所有方块越级为0，开启   
		MotionBlocksControl(true);   
		   
		switch(MissileCommand){   
			case ("Attack"):   
				Attack();   
			break;   
			case ("Die"):   
				Die();   
			break;   
			case ("Wing"):   
				Wing();   
			break;   
			case ("Sentry"):   
				Sentry();   
			break;   
			case ("Dock"):   
				Dock();   
			break;   
			case ("Stop"):   
			   
			break;   
		}   
		   
		//显示天线   
		if(Antenna != null){   
			Antenna.ApplyAction("OnOff_Off");   
			string MeID = "EZ #" +  arguments.Split('[')[1].Split(']')[0];   
			Antenna.CustomName = MeID;   
			if(MissileCommand != "Attack"){   
				if(Vector3D.Distance(MePosition, MotherCockpit.GetPosition()) <= Antenna.Radius){   
					Antenna.ApplyAction("OnOff_On");   
				}   
			}   
		}   
		   
		Echo("当前指令: " + MissileCommand);   
		//末更新制导数据   
		NavigationInfoUpdata(false, arguments);   
	}   
}   
   
// ============== 飞船指令类 ====================   
// ------------ 攻击函数 -------------   
void Attack()   
{   
	//目标不存在，停机。启动发射程序   
	if(TargetPosition.Length() == 0 || !ShipDetach(true)){   
		return;   
	}   
	if(!LaunchControl(LaunchPoint)){return;}   
	   
	//检测距离，获得必要参数   
	double DistanceToTarget = Vector3D.Distance(MePosition,TargetPosition);   
	MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), Cockpit.WorldMatrix.Forward, Cockpit.WorldMatrix.Up);   
	   
	//使用这两个参数控制所有运动   
	Vector3D TargetPositionToMe = new Vector3D();   
	Vector3D TargetVelecityToMe = new Vector3D();   
	   
	Vector3D MeVelocityToMe = Vector3D.TransformNormal(MeVelocity, refLookAtMatrix);   
	   
	//巡航接近   
	if(DistanceToTarget >= 1.5*CombatRadius){   
		AimAtTarget(TargetPosition);   
		   
		TargetPositionToMe = Vector3D.TransformNormal(TargetPosition - MePosition, refLookAtMatrix);   
		TargetVelecityToMe = Vector3D.TransformNormal(TargetVelecity, refLookAtMatrix);   
		//执行运动控制   
		SpeedControl("Forward", "Backward", MeVelocityToMe.Z, TargetPositionToMe.Z+TargetVelecityToMe.Z);   
		SpeedControl("Left", "Right", MeVelocityToMe.X, TargetPositionToMe.X+TargetVelecityToMe.X);   
		SpeedControl("Down", "Up", MeVelocityToMe.Y, TargetPositionToMe.Y+TargetVelecityToMe.Y);   
	}   
	//俯冲轰炸   
	else if(DistanceToTarget >= CombatRadius){   
		//瞄准目标
		AimAtTarget(TargetPosition, TargetVelecity, TargetAcceleration, BulletInitSpeed, BulletAcceleration, BulletMaxSpeed);   
		   
		//如果是静止目标，导航到800米附近震荡   
		if(TargetVelecity.Length() == 0){   
			TargetPositionToMe = Vector3D.TransformNormal((TargetPosition + new Vector3D(500,500,500)) - MePosition, refLookAtMatrix);   
		}   
		else{//运动目标导航到目标前方900米   
			TargetPositionToMe = Vector3D.TransformNormal((TargetPosition + Vector3D.Normalize(TargetVelecity)*528 ) - MePosition, refLookAtMatrix);   
		}   
		   
		TargetVelecityToMe = Vector3D.TransformNormal(TargetVelecity, refLookAtMatrix);   
		//执行运动控制   
		SpeedControl("Forward", "Backward", MeVelocityToMe.Z, TargetPositionToMe.Z+TargetVelecityToMe.Z);   
		SpeedControl("Left", "Right", MeVelocityToMe.X, TargetPositionToMe.X+TargetVelecityToMe.X);   
		SpeedControl("Down", "Up", MeVelocityToMe.Y, TargetPositionToMe.Y+TargetVelecityToMe.Y);   
	}   
	//规避   
	else{   
		AimAtTarget(TargetPosition, TargetVelecity, TargetAcceleration, BulletInitSpeed, BulletAcceleration, BulletMaxSpeed);  
		SetThrustOverride("Forward",100);   
		SetThrustOverride("Backward", 0);   
		MoveRoll();   
	}   
	   
	//作战高阶动作   
	if(ActionBlockNameTag.Count() == ActionDistanceToTarget.Count() && ActionBlockNameTag.Count() == ActionName.Count())   
	{   
		for(int i = 0; i < ActionBlockNameTag.Count(); i ++)   
		{   
			if(Vector3D.Distance(TargetPosition,MePosition) <= ActionDistanceToTarget[i])   
			{   
				IMyTerminalBlock action_block = GridTerminalSystem.GetBlockWithName(ActionBlockNameTag[i]);   
				if(action_block!=null)   
				{action_block.ApplyAction(ActionName[i]);}   
			}   
		}   
	}   
}   
   
// ------------ 神风函数 ------------   
void Die()   
{   
	//目标不存在，停机。启动发射程序   
	if(TargetPosition.Length() == 0 || !ShipDetach(true)){   
		return;   
	}   
	if(!LaunchControl(LaunchPoint)){return;}   
	   
	MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), Cockpit.WorldMatrix.Forward, Cockpit.WorldMatrix.Up);   
	Vector3D TargetPositionToMe = Vector3D.TransformNormal(TargetPosition - MePosition, refLookAtMatrix);   
	Vector3D MeVelocityToMe = Vector3D.TransformNormal(MeVelocity, refLookAtMatrix);   
	Vector3D TargetVelecityToMe = Vector3D.TransformNormal(TargetVelecity, refLookAtMatrix);   
	   
	//执行运动控制   
	SpeedControl("Forward", "Backward", MeVelocityToMe.Z, TargetPositionToMe.Z+TargetVelecityToMe.Z);   
	SpeedControl("Left", "Right", MeVelocityToMe.X, TargetPositionToMe.X+TargetVelecityToMe.X);   
	SpeedControl("Down", "Up", MeVelocityToMe.Y, TargetPositionToMe.Y+TargetVelecityToMe.Y);   
	
	//瞄准目标
	AimAtTarget(TargetPosition, TargetVelecity, TargetAcceleration, BulletInitSpeed, BulletAcceleration, BulletMaxSpeed);
	   
	//作战高阶动作   
	if(ActionBlockNameTag.Count() == ActionDistanceToTarget.Count() && ActionBlockNameTag.Count() == ActionName.Count())   
	{   
		for(int i = 0; i < ActionBlockNameTag.Count(); i ++)   
		{   
			if(Vector3D.Distance(TargetPosition,MePosition) <= ActionDistanceToTarget[i])   
			{   
				IMyTerminalBlock action_block = GridTerminalSystem.GetBlockWithName(ActionBlockNameTag[i]);   
				if(action_block!=null)   
				{action_block.ApplyAction(ActionName[i]);}   
			}   
		}   
	}   
}   
   
// ------------ 伴飞函数 ------------   
void Wing()   
{   
	if(!ShipDetach(true)){return;}   
	if(!LaunchControl(LaunchPoint)){return;}   
   
	//速度导航控制   
	Vector3D WingPoint = MotherCockpit.GetPosition() + MotherCockpit.WorldMatrix.Right*WingFlyPosition.X + MotherCockpit.WorldMatrix.Up*WingFlyPosition.Y + MotherCockpit.WorldMatrix.Backward*WingFlyPosition.Z;   
	if(WingFlyDistanceMultiple != 0){   
		WingPoint = MotherCockpit.GetPosition() + (MotherConnector.GetPosition() - MotherCockpit.GetPosition())*WingFlyDistanceMultiple;   
	}   
	   
	//碰撞避免   
		//计算母船在本无人机航线上的投影   
	Vector3D MotherProjectionPoint = RouteVerticalPoint(MotherCockpit.GetPosition(), MePosition, MeVelocity+MePosition);   
		//启用避撞的条件   
	if(Vector3D.Distance(MotherProjectionPoint, MotherCockpit.GetPosition()) <= SafeDistanceToMotherShip //母船在速度线的投影点与母船本体距离小于安全距离   
		//&& (MeVelocity.Length()*(MeVelocity.Length()/MeAcceleration.Length()) + 0.5*MeAcceleration.Length()*Math.Pow(MeVelocity.Length()/MeAcceleration.Length(), 2)) <= Vector3D.Distance(MePosition, MotherCockpit.GetPosition()) //以无人机当前的速度与加速度判断会撞击母船   
		&& (MeVelocity.Length()*(MeVelocity.Length()/MeAcceleration.Length()) + 0.5*Vector3D.Dot(MeAcceleration,MeVelocity)*Math.Pow(MeVelocity.Length()/MeAcceleration.Length(), 2)) <= Vector3D.Distance(MePosition, MotherCockpit.GetPosition()) //以无人机当前的速度与加速度判断会撞击母船   
		&& Vector3D.Dot((MotherCockpit.GetPosition() - MePosition), MeVelocity) > 0 //无人机正在接近母船   
		&& (MotherCockpit.GetShipVelocities().LinearVelocity - MeVelocity).Length() > 10) //无人机与母船相对速度大于10   
	{   
		//强制导航到新的坐标点，伴飞点外1000米处   
		WingPoint = MotherCockpit.GetPosition() + 1000*(MotherCockpit.WorldMatrix.Right*WingFlyPosition.X + MotherCockpit.WorldMatrix.Up*WingFlyPosition.Y + MotherCockpit.WorldMatrix.Backward*WingFlyPosition.Z);   
	}   
	   
	//执行伴飞速度控制   
	WingFlyControl(WingPoint, MotherCockpit.GetShipVelocities().LinearVelocity);   
	   
	//指向控制   
	Vector3D MotherAimPoint = MotherCockpit.WorldMatrix.Forward*100000000;   
	if(AimAtTarget(MotherAimPoint)){   
		double Y_Angle = Vector3D.Dot(MotherCockpit.WorldMatrix.Up, Cockpit.WorldMatrix.Up);   
		double X_Angle = Vector3D.Dot(MotherCockpit.WorldMatrix.Left, Cockpit.WorldMatrix.Up);   
		   
		if(X_Angle > 0){   
			SetGyroRoll((Y_Angle+1)*60*GyroscopesControlRatio);   
		}   
		else{   
			SetGyroRoll(-(Y_Angle+1)*60*GyroscopesControlRatio);   
		}
	}   
	   
}   
   
// -------- 停泊函数 -------------   
double DockStep;   
void Dock()   
{   
	if(ShipDetach(false)){   
		if(!HelpMotherFly){   
			MotionBlocksControl(false);//关闭所有运动方块   
		}   
		else{   
			MotherFlyControl();   
		}   
		return;   
	}   
	if(!LaunchControl(LaunchPoint)){return;}   
	   
	//指向控制   
	MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), Cockpit.WorldMatrix.Forward, Cockpit.WorldMatrix.Up);   
	   
	if(AimAtTarget(MotherCockpit.WorldMatrix.Forward*100000000)){   
		double Y_Angle = Vector3D.Dot(MotherConnector.WorldMatrix.Forward, DetachBlock[0].WorldMatrix.Forward);   
		double X_Angle = Vector3D.Dot(Vector3D.Cross(MotherConnector.WorldMatrix.Forward, DetachBlock[0].WorldMatrix.Forward), MotherCockpit.WorldMatrix.Forward); //把两个向量的叉乘，与母船远程控制块前向量进行点乘，从而判断   
   
		if(X_Angle < 0){   
			SetGyroRoll((Y_Angle+1)*60*GyroscopesControlRatio);   
		}   
		else{   
			SetGyroRoll(-(Y_Angle+1)*60*GyroscopesControlRatio);   
		} 
	}   
	   
		//计算座标   
	Vector3D MeConnectorToCockpitVector = Cockpit.GetPosition() - DetachBlock[0].GetPosition();   
	Vector3D DockPoint = MotherConnector.GetPosition() + MotherConnector.WorldMatrix.Forward*DockStep + MeConnectorToCockpitVector;   
	if(t%DockSpeed == 0 && DockStep > 1){   
		DockStep -= 0.8;   
	}   
	   
	WingFlyControl(DockPoint, MotherCockpit.GetShipVelocities().LinearVelocity);   
}   
   
// -------- 哨岗函数 -------------   
void Sentry()   
{   
	//检查分离   
	if(!ShipDetach(true)){return;}   
	if(!LaunchControl(LaunchPoint)){return;}   
	   
	//速度导航控制   
	Vector3D WingPoint = MotherCockpit.GetPosition() + MotherCockpit.WorldMatrix.Right*WingFlyPosition.X + MotherCockpit.WorldMatrix.Up*WingFlyPosition.Y + MotherCockpit.WorldMatrix.Backward*WingFlyPosition.Z;   
	if(WingFlyDistanceMultiple != 0){   
		WingPoint = MotherCockpit.GetPosition() + (MotherConnector.GetPosition() - MotherCockpit.GetPosition())*WingFlyDistanceMultiple;   
	}   
	   
	//碰撞避免   
		//计算母船在本无人机航线上的投影   
	Vector3D MotherProjectionPoint = RouteVerticalPoint(MotherCockpit.GetPosition(), MePosition, MeVelocity+MePosition);   
		//启用避撞的条件   
	if(Vector3D.Distance(MotherProjectionPoint, MotherCockpit.GetPosition()) <= SafeDistanceToMotherShip //母船在速度线的投影点与母船本体距离小于安全距离   
		//&& (MeVelocity.Length()*(MeVelocity.Length()/MeAcceleration.Length()) + 0.5*MeAcceleration.Length()*Math.Pow(MeVelocity.Length()/MeAcceleration.Length(), 2)) <= Vector3D.Distance(MePosition, MotherCockpit.GetPosition()) //以无人机当前的速度与加速度判断会撞击母船   
		&& (MeVelocity.Length()*(MeVelocity.Length()/MeAcceleration.Length()) + 0.5*Vector3D.Dot(MeAcceleration,MeVelocity)*Math.Pow(MeVelocity.Length()/MeAcceleration.Length(), 2)) <= Vector3D.Distance(MePosition, MotherCockpit.GetPosition()) //以无人机当前的速度与加速度判断会撞击母船   
		&& Vector3D.Dot((MotherCockpit.GetPosition() - MePosition), MeVelocity) > 0) //无人机正在接近母船   
	{   
		//强制导航到新的坐标点，伴飞点外1000米处   
		WingPoint = MotherCockpit.GetPosition() + 1000*(MotherCockpit.WorldMatrix.Right*WingFlyPosition.X + MotherCockpit.WorldMatrix.Up*WingFlyPosition.Y + MotherCockpit.WorldMatrix.Backward*WingFlyPosition.Z);   
	}   
	   
	//执行伴飞速度控制   
	WingFlyControl(WingPoint, MotherCockpit.GetShipVelocities().LinearVelocity);   
	   
	//检查目标存在   
	if(TargetPosition.Length() != 0){   
		//瞄准目标
		AimAtTarget(TargetPosition, TargetVelecity, TargetAcceleration, BulletInitSpeed, BulletAcceleration, BulletMaxSpeed);   
		   
		//判断开火条件   
		if(Vector3D.Distance(RouteVerticalPoint(MotherCockpit.CenterOfMass, MePosition, MePosition+Cockpit.WorldMatrix.Forward*1000), MotherCockpit.CenterOfMass) >= SafeDistanceToMotherShip   
			|| Vector3D.Dot((Cockpit.WorldMatrix.Forward*1000), (MotherCockpit.CenterOfMass - MePosition)) <= 0   
		){   
			//执行作战高阶动作   
			if(ActionBlockNameTag.Count() == ActionDistanceToTarget.Count() && ActionBlockNameTag.Count() == ActionName.Count())   
			{   
				for(int i = 0; i < ActionBlockNameTag.Count(); i ++)   
				{   
					if(Vector3D.Distance(TargetPosition,MePosition) <= ActionDistanceToTarget[i])   
					{   
						IMyTerminalBlock action_block = GridTerminalSystem.GetBlockWithName(ActionBlockNameTag[i]);   
						if(action_block!=null)   
						{action_block.ApplyAction(ActionName[i]);}   
					}   
				}   
			}   
		}   
	}   
	else{   
		Vector3D MotherAimPoint = MotherCockpit.WorldMatrix.Forward*1000000;   
		if(AimAtTarget(MotherAimPoint)){   
			double Y_Angle = Vector3D.Dot(MotherCockpit.WorldMatrix.Up, Cockpit.WorldMatrix.Up);   
			double X_Angle = Vector3D.Dot(MotherCockpit.WorldMatrix.Left, Cockpit.WorldMatrix.Up);   
			   
			if(X_Angle > 0){   
				SetGyroRoll((Y_Angle+1)*60*GyroscopesControlRatio);   
			}   
			else{   
				SetGyroRoll(-(Y_Angle+1)*60*GyroscopesControlRatio);   
			}  
		}   
	}   
}   
   
// -------- 辅助飞行函数 ------------   
void MotherFlyControl()   
{   
	Cockpit.DampenersOverride = MotherCockpit.DampenersOverride;   
	Vector3 WASD = MotherCockpit.MoveIndicator;   
	   
	Base6Directions.Direction CockpitForward = Cockpit.WorldMatrix.GetClosestDirection(MotherCockpit.WorldMatrix.Forward);   
	Base6Directions.Direction CockpitUp = Cockpit.WorldMatrix.GetClosestDirection(MotherCockpit.WorldMatrix.Up);   
	Base6Directions.Direction CockpitLeft = Cockpit.WorldMatrix.GetClosestDirection(MotherCockpit.WorldMatrix.Left);   
	switch (CockpitForward){    
		case Base6Directions.Direction.Forward:   
		SetThrustOverride("Forward", WASD.Z*100);   
		SetThrustOverride("Backward", -WASD.Z*100);   
		break;    
		case Base6Directions.Direction.Backward:    
		SetThrustOverride("Forward", -WASD.Z*100);   
		SetThrustOverride("Backward", WASD.Z*100);   
		break;    
		case Base6Directions.Direction.Left:    
		SetThrustOverride("Right", WASD.Z*100);   
		SetThrustOverride("Left", -WASD.Z*100);   
		break;   
		case Base6Directions.Direction.Right:    
		SetThrustOverride("Right", -WASD.Z*100);   
		SetThrustOverride("Left", WASD.Z*100);   
		break;   
		case Base6Directions.Direction.Up:    
		SetThrustOverride("Up", WASD.Z*100);   
		SetThrustOverride("Down", -WASD.Z*100);   
		break;   
		case Base6Directions.Direction.Down:    
		SetThrustOverride("Up", -WASD.Z*100);   
		SetThrustOverride("Down", WASD.Z*100);   
		break;   
	}   
	switch (CockpitUp){    
		case Base6Directions.Direction.Forward:   
		SetThrustOverride("Forward", WASD.Y*100);   
		SetThrustOverride("Backward", -WASD.Y*100);   
		break;    
		case Base6Directions.Direction.Backward:    
		SetThrustOverride("Forward", -WASD.Y*100);   
		SetThrustOverride("Backward", WASD.Y*100);   
		break;    
		case Base6Directions.Direction.Left:    
		SetThrustOverride("Right", WASD.Y*100);   
		SetThrustOverride("Left", -WASD.Y*100);   
		break;   
		case Base6Directions.Direction.Right:    
		SetThrustOverride("Right", -WASD.Y*100);   
		SetThrustOverride("Left", WASD.Y*100);   
		break;   
		case Base6Directions.Direction.Up:    
		SetThrustOverride("Up", WASD.Y*100);   
		SetThrustOverride("Down", -WASD.Y*100);   
		break;   
		case Base6Directions.Direction.Down:    
		SetThrustOverride("Up", -WASD.Y*100);   
		SetThrustOverride("Down", WASD.Y*100);   
		break;   
	}   
	switch (CockpitLeft){    
		case Base6Directions.Direction.Forward:   
		SetThrustOverride("Forward", WASD.X*100);   
		SetThrustOverride("Backward", -WASD.X*100);   
		break;    
		case Base6Directions.Direction.Backward:    
		SetThrustOverride("Forward", -WASD.X*100);   
		SetThrustOverride("Backward", WASD.X*100);   
		break;    
		case Base6Directions.Direction.Left:    
		SetThrustOverride("Right", WASD.X*100);   
		SetThrustOverride("Left", -WASD.X*100);   
		break;   
		case Base6Directions.Direction.Right:    
		SetThrustOverride("Right", -WASD.X*100);   
		SetThrustOverride("Left", WASD.X*100);   
		break;   
		case Base6Directions.Direction.Up:    
		SetThrustOverride("Up", WASD.X*100);   
		SetThrustOverride("Down", -WASD.X*100);   
		break;   
		case Base6Directions.Direction.Down:    
		SetThrustOverride("Up", -WASD.X*100);   
		SetThrustOverride("Down", WASD.X*100);   
		break;   
	}   
}   
   
// ============================== 动作控制类 ==================================   
// true表示分离，false表示关闭，同时执行对应操作，执行成功返回true   
bool ShipDetach(bool onoff)   
{   
	if(onoff){   
		bool success = true;   
		for(int i = 0; i < DetachBlock.Count; i++){   
			switch (DetachBlockType[i])   
				{   
				case 0:   
					IMyFunctionalBlock block_a = DetachBlock[i] as IMyFunctionalBlock;   
					if(block_a.Enabled){success = false;}   
					break;   
				case 1:   
					IMyShipConnector block_b = DetachBlock[i] as IMyShipConnector;   
					if(block_b.Status == MyShipConnectorStatus.Connected){success = false;}   
					break;   
				}   
		}   
		if(success){return true;}   
		   
		for(int i = 0; i < DetachBlock.Count; i++){   
			if(DetachBlock[i] != null)   
			{   
				switch (DetachBlockType[i])   
				{   
				case 0:   
					DetachBlock[i].ApplyAction("OnOff_Off");   
					break;   
				case 1:   
					IMyShipConnector DetachBlock_1 = DetachBlock[i] as IMyShipConnector;   
					DetachBlock_1.Disconnect();   
					break;   
				}   
			}   
			else{   
				Debug = "分离方块: " + DetachBlockNameTag[i] + " 未找到.";   
				return false;   
			}   
		}   
	}   
	else{   
		bool success = true;   
		for(int i = 0; i < DetachBlock.Count; i++){   
			switch (DetachBlockType[i])   
				{   
				case 0:   
					IMyFunctionalBlock block_a = DetachBlock[i] as IMyFunctionalBlock;   
					if(!block_a.Enabled){success = false;}   
					break;   
				case 1:   
					IMyShipConnector block_b = DetachBlock[i] as IMyShipConnector;   
					if(block_b.Status != MyShipConnectorStatus.Connected){success = false;}   
					break;   
				}   
		}   
		if(success){return true;}   
		for(int i = 0; i < DetachBlock.Count; i++){   
			if(DetachBlock[i] != null)   
			{   
				switch (DetachBlockType[i])   
				{   
				case 0:   
					DetachBlock[i].ApplyAction("OnOff_On");   
					break;   
				case 1:   
					IMyShipConnector DetachBlock_1 = DetachBlock[i] as IMyShipConnector;   
					DetachBlock_1.Connect();   
					break;   
				}   
			}   
			else{   
				Debug = "分离方块: " + DetachBlockNameTag[i] + " 未找到.";   
				return false;   
			}   
		}   
	}   
	return false;   
}   
   
// ============================== 运动控制类 ====================================   
// ------------- 运动控制初始化 ----------   
void MotionBlocksControl(bool onoff)   
{   
	Cockpit.DampenersOverride = true;   
	SetGyroYaw(0);   
	SetGyroPitch(0);   
	SetGyroRoll(0);   
	SetThrustOverride("All",0f);   
	SetGyroOverride(false);   
	if(onoff){   
		//初始化方块动作   
		//Cockpit.DampenersOverride = false;   
		TurnBlocksOn(Thrusts,true);   
		TurnBlocksOn(Gyroscopes,true);   
	}   
	else{   
		//初始化方块动作   
		//Cockpit.DampenersOverride = false;   
		TurnBlocksOn(Thrusts,false);   
		TurnBlocksOn(Gyroscopes,false);   
	}   
	if(MissileCommand != "Dock"){   
		DockStep = DockDistance;   
	}   
}   
   
// ------------- 起飞控制函数 ----------   
// 在分离函数中重置isLaunched变量   
bool isLaunched;   
double LaunchControlRatio = 1; //停泊点判定精度   
bool LaunchControl(Vector3D Point)   
{   
	if(Point.Length() == 0){   
		return true;   
	}   
	if(!isLaunched && init >= 3){   
		Vector3D LaunchPoint_Ab = MotherCockpit.GetPosition() + MotherCockpit.WorldMatrix.Right*Point.X + MotherCockpit.WorldMatrix.Up*Point.Y + MotherCockpit.WorldMatrix.Backward*Point.Z;   
		WingFlyControl(LaunchPoint_Ab, MotherCockpit.GetShipVelocities().LinearVelocity);   
		if(Vector3D.Distance(MePosition, LaunchPoint_Ab) <= LaunchControlRatio){   
			isLaunched = true;   
		}   
		if(isLaunched){return true;}   
		else{return false;}   
	}   
	return true;   
}   
   
// ------------- 伴飞控制函数 ----------   
//给出伴飞点绝对座标，伴飞点运动的绝对速度。控制飞船伴飞   
void WingFlyControl(Vector3D WingPoint, Vector3D WingPointVelocity)   
{   
	MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), Cockpit.WorldMatrix.Forward, Cockpit.WorldMatrix.Up);   
	   
	Vector3D WingPointToMe = Vector3D.TransformNormal(WingPoint - MePosition, refLookAtMatrix);   
   
		//计算速度   
	Vector3D WingPointVelocityToMe = Vector3D.TransformNormal(WingPointVelocity, refLookAtMatrix);   
	Vector3D MeVelocityToMe = Vector3D.TransformNormal(MeVelocity, refLookAtMatrix);   
	   
		//执行运动控制   
	SpeedControl("Forward", "Backward", MeVelocityToMe.Z, WingPointToMe.Z+WingPointVelocityToMe.Z);   
	SpeedControl("Left", "Right", MeVelocityToMe.X, WingPointToMe.X+WingPointVelocityToMe.X);   
	SpeedControl("Down", "Up", MeVelocityToMe.Y, WingPointToMe.Y+WingPointVelocityToMe.Y);   
}   
   
// --------------- 螺旋机动控制函数 ------------   
int Roll_I = 1; //圆周运动象限指示变量   
double Roll_X; //圆周运动所需的自增变量   
double Roll_Y;   
void MoveRoll()   
{   
	if(Roll_I == 1){   
		Roll_X += Roll_Speed;   
		Roll_Y = Math.Pow( Math.Pow(Roll_R,2) - Math.Pow(Roll_X,2) , 0.5);   
		if(Roll_X > Roll_R){   
			Roll_I = 2;   
		}   
	}   
	else if(Roll_I == 2){   
		Roll_X -= Roll_Speed;   
		Roll_Y = -Math.Pow( Math.Pow(Roll_R,2) - Math.Pow(Roll_X,2) , 0.5);   
		if(Roll_X < 0){   
			Roll_I = 3;   
		}   
	}   
	else if(Roll_I == 3){   
		Roll_X -= Roll_Speed;   
		Roll_Y = -Math.Pow( Math.Pow(Roll_R,2) - Math.Pow(Roll_X,2) , 0.5);   
		if(Roll_X < -Roll_R){   
			Roll_I = 4;   
		}   
	}   
	else if(Roll_I == 4){   
		Roll_X += Roll_Speed;   
		Roll_Y = Math.Pow( Math.Pow(Roll_R,2) - Math.Pow(Roll_X,2) , 0.5);   
		if(Roll_X > 0){   
			Roll_I = 1;   
		}   
	}   
	if(Roll_X > 0){   
		SetThrustOverride("Left",Roll_X);   
		SetThrustOverride("Right",0);   
	}   
	else{   
		SetThrustOverride("Left",0);   
		SetThrustOverride("Right",-Roll_X);   
	}   
	if(Roll_Y > 0){   
		SetThrustOverride("Up",Roll_Y);   
		SetThrustOverride("Down",0);   
	}   
	else{   
		SetThrustOverride("Up",0);   
		SetThrustOverride("Down",-Roll_Y);   
	}   
}   

// --------------- PID瞄准目标函数 -------------
private List<Vector3D> Aim_PID_Data = new List<Vector3D>();
bool AimAtTarget(Vector3D Position)
{
	return AimAtTarget(Position, new Vector3D(), new Vector3D(), 100000, 0, 100000);
}

bool AimAtTarget(Vector3D Position, Vector3D Velocity, Vector3D Acceleration)
{
	return AimAtTarget(Position, Velocity, Acceleration, 100000, 0, 100000);
}

bool AimAtTarget(Vector3D Position, Vector3D Velocity, Vector3D Acceleration, double BulletInitialSpeed, double BulletAcceleration, double BulletMaxSpeed)
{
	MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), Cockpit.WorldMatrix.Forward, Cockpit.WorldMatrix.Up);
	Vector3D HitPoint = HitPointCaculate(MePosition, MeVelocity, MeAcceleration, Position, Velocity, Acceleration, BulletInitialSpeed, BulletAcceleration, BulletMaxSpeed);
	Vector3D HitPositionToMe = Vector3D.Normalize(Vector3D.TransformNormal(HitPoint - Cockpit.GetPosition(), refLookAtMatrix));
	
	//储存采样点
	if(Aim_PID_Data.Count < Aim_PID_T){
		for(int i = 0; i < Aim_PID_T; i ++){
			Aim_PID_Data.Add(new Vector3D());
		}
	}
	else{Aim_PID_Data.Remove(Aim_PID_Data[0]); Aim_PID_Data.Add(HitPositionToMe);}
	
	//获得采样点积分
	double X_I = 0;
	double Y_I = 0;
	foreach(Vector3D datapoint in Aim_PID_Data){
		X_I += datapoint.X;
		Y_I += datapoint.Y;
	}
	
	//计算输出
	double YawValue = AimPID_P*(HitPositionToMe.X + (1/AimPID_I)*X_I + AimPID_D*(Aim_PID_Data[Aim_PID_T-1].X - Aim_PID_Data[0].X)/Aim_PID_T) * 60;
	double PitchValue = AimPID_P*(HitPositionToMe.Y + (1/AimPID_I)*Y_I + AimPID_D*(Aim_PID_Data[Aim_PID_T-1].Y - Aim_PID_Data[0].Y)/Aim_PID_T) * 60;
	SetGyroYaw(YawValue);
	SetGyroPitch(PitchValue);
	SetGyroOverride(true); 
	
	// 计算当前与预期瞄准点的瞄准夹角
	Vector3D V_A = HitPoint - MePosition;
	Vector3D V_B = Cockpit.WorldMatrix.Forward;
	double Angle = Math.Acos(Vector3D.Dot(V_A,V_B)/(V_A.Length() * V_B.Length())) * 180 / Math.PI;
	if(Angle <= AimRatio) return true;
	return false;
}  
   
// ------------- 坐标点导航函数 --------------   
double SafeBreakDistance = 800; //安全刹车距离   
bool PointGuaid(Vector3D TargetPosition, bool NeedFaceTo)   
{   
	MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), Cockpit.WorldMatrix.Forward, Cockpit.WorldMatrix.Up);   
	Vector3D PositionToMe = Vector3D.TransformNormal(TargetPosition - Cockpit.GetPosition(), refLookAtMatrix);   
	Vector3D MeVelocityToMe = Vector3D.TransformNormal(Cockpit.GetShipVelocities().LinearVelocity, refLookAtMatrix);   
	SetThrustOverride("All",0);//初始化推进器   
	if(NeedFaceTo)   
	{   
		SpeedControl("Right", "Left", -MeVelocityToMe.X, 0);   
		SpeedControl("Down", "Up", MeVelocityToMe.Y, 0);   
		if(AimAtTarget(TargetPosition))   
		{   
			if(PositionToMe.Length() >= SafeBreakDistance)   
			{   
				SpeedControl("Backward", "Forward", -MeVelocityToMe.Z, -PositionToMe.Z);   
			}   
			else if(PositionToMe.Length() >= 1 && SpeedControl("Backward", "Forward", -MeVelocityToMe.Z, -PositionToMe.Z*100/SafeBreakDistance))   
			{   
				SpeedControl("Backward", "Forward", -MeVelocityToMe.Z, 0);   
				return true;   
			}   
		}   
	}   
	else   
	{   
		if(PositionToMe.Length() >= SafeBreakDistance)   
		{   
			SpeedControl("Backward", "Forward", -MeVelocityToMe.Z, -PositionToMe.Z);   
			SpeedControl("Right", "Left", -MeVelocityToMe.X, -PositionToMe.X);   
			SpeedControl("Down", "Up", MeVelocityToMe.Y, PositionToMe.Y);   
		}   
		else if(PositionToMe.Length() >= 1)   
		{   
			SpeedControl("Backward", "Forward", -MeVelocityToMe.Z, -PositionToMe.Z*100/SafeBreakDistance);   
			SpeedControl("Right", "Left", -MeVelocityToMe.X, -PositionToMe.X*100/SafeBreakDistance);   
			SpeedControl("Down", "Up", MeVelocityToMe.Y, PositionToMe.Y*100/SafeBreakDistance);   
		}   
		else return true;   
	}   
	return false;   
}   
   
// ----------------- 单向速度控制函数 -----------   
bool SpeedControl(string PositiveThrusts, string NagtiveThrusts, double NowSpeed, double TargetSpeed)   
{   
	if(Math.Abs(NowSpeed) <= 1 && Math.Abs(TargetSpeed) <= 0.01)   
	{   
		SetThrustOverride(PositiveThrusts, 0);   
		SetThrustOverride(NagtiveThrusts, 0);   
		return true;   
	}   
	Random rd = new Random(t);   
	if(NowSpeed < TargetSpeed)   
	{   
		SetThrustOverride(PositiveThrusts, (Math.Abs(TargetSpeed - NowSpeed) <= 1 ? rd.Next(10,80) : 100));   
		SetThrustOverride(NagtiveThrusts, 0);   
	}   
	else if(NowSpeed > TargetSpeed)   
	{   
		SetThrustOverride(PositiveThrusts, 0);   
		SetThrustOverride(NagtiveThrusts, (Math.Abs(TargetSpeed - NowSpeed) <= 1 ? rd.Next(10,80) : 100));   
	}   
	else   
	{   
		SetThrustOverride(PositiveThrusts, 0);   
		SetThrustOverride(NagtiveThrusts, 0);   
	}   
	return false;   
}   
   
// ============================== 信息处理类 ===================================   
// -------------------- 导航数据更新函数 --------------------------   
// 主要更新自己和目标的加速度，当传入false时更新座标及速度，传入true时更新加速度   
Vector3D MePosition;   
Vector3D MeLastPosition;   
Vector3D MeVelocity;   
Vector3D MeLastVelocity;   
Vector3D MeAcceleration;   
Vector3D TargetPosition;   
Vector3D TargetLastPosition;   
Vector3D TargetVelecity;   
Vector3D TargetLastVelecity;   
Vector3D TargetAcceleration;   
double TargetDiameter;   
void NavigationInfoUpdata(bool before, string arguments)   
{   
	if(before)   
	{   
		MePosition = Cockpit.GetPosition();   
		MeVelocity = Cockpit.GetShipVelocities().LinearVelocity;   
		MeAcceleration = (MeVelocity-MeLastVelocity)*60;   
		   
		Vector3D.TryParse(arguments.Split('@')[2], out TargetPosition);   
		Vector3D.TryParse(arguments.Split('@')[3], out TargetVelecity);   
		double.TryParse(arguments.Split('@')[4], out TargetDiameter);   
		TargetAcceleration =(TargetVelecity-TargetLastVelecity)*60;   
	}   
	else   
	{   
		MeLastPosition = Cockpit.GetPosition();   
		MeLastVelocity = Cockpit.GetShipVelocities().LinearVelocity;   
		   
		Vector3D.TryParse(arguments.Split('@')[2], out TargetLastPosition);   
		Vector3D.TryParse(arguments.Split('@')[3], out TargetLastVelecity);   
		double.TryParse(arguments.Split('@')[4], out TargetDiameter);   
	}   
}   
   
// ============= 运算类函数 =====================   
//计算碰撞点的函数，分别传入：飞船座标、飞船速度、飞船加速度、目标座标、目标速度、目标加速度、子弹初速度、子弹加速度、子弹末速度       
Vector3D HitPointCaculate(Vector3D Me_Position, Vector3D Me_Velocity, Vector3D Me_Acceleration, Vector3D Target_Position, Vector3D Target_Velocity, Vector3D Target_Acceleration,        
							double Bullet_InitialSpeed, double Bullet_Acceleration, double Bullet_MaxSpeed)       
{       
	//迭代算法       
	Vector3D HitPoint = new Vector3D();       
	Vector3D Smt = Target_Position - Me_Position;//发射点指向目标的矢量       
	Vector3D Velocity = Target_Velocity - Me_Velocity; //目标飞船和自己飞船总速度       
	Vector3D Acceleration = Target_Acceleration; //目标飞船和自己飞船总加速度       
	       
	double AccTime = (Bullet_Acceleration == 0 ? 0 : (Bullet_MaxSpeed - Bullet_InitialSpeed)/Bullet_Acceleration);//子弹加速到最大速度所需时间       
	double AccDistance = Bullet_InitialSpeed*AccTime + 0.5*Bullet_Acceleration*AccTime*AccTime;//子弹加速到最大速度经过的路程       
	       
	double HitTime = 0;       
	       
	if(AccDistance < Smt.Length())//目标在炮弹加速过程外       
	{       
		HitTime = (Smt.Length() - Bullet_InitialSpeed*AccTime - 0.5*Bullet_Acceleration*AccTime*AccTime + Bullet_MaxSpeed*AccTime)/Bullet_MaxSpeed;       
		HitPoint = Target_Position + Velocity*HitTime + 0.5*Acceleration*HitTime*HitTime;       
	}       
	else//目标在炮弹加速过程内     
	{       
		double HitTime_Z = (-Bullet_InitialSpeed + Math.Pow((Bullet_InitialSpeed*Bullet_InitialSpeed + 2*Bullet_Acceleration*Smt.Length()),0.5))/Bullet_Acceleration;       
		double HitTime_F = (-Bullet_InitialSpeed - Math.Pow((Bullet_InitialSpeed*Bullet_InitialSpeed + 2*Bullet_Acceleration*Smt.Length()),0.5))/Bullet_Acceleration;       
		HitTime = (HitTime_Z > 0 ? (HitTime_F > 0 ? (HitTime_Z < HitTime_F ? HitTime_Z : HitTime_F) : HitTime_Z) : HitTime_F);       
		HitPoint = Target_Position + Velocity*HitTime + 0.5*Acceleration*HitTime*HitTime;       
	}       
	//迭代，仅迭代更新碰撞时间，每次迭代更新右5位数量级       
	for(int i = 0; i < 3; i ++)       
	{       
		if(AccDistance < Vector3D.Distance(HitPoint,Me_Position))//目标在炮弹加速过程外       
		{       
			HitTime = (Vector3D.Distance(HitPoint,Me_Position) - Bullet_InitialSpeed*AccTime - 0.5*Bullet_Acceleration*AccTime*AccTime + Bullet_MaxSpeed*AccTime)/Bullet_MaxSpeed;       
			HitPoint = Target_Position + Velocity*HitTime + 0.5*Acceleration*HitTime*HitTime;       
		}       
		else//目标在炮弹加速过程内       
		{       
			double HitTime_Z = (-Bullet_InitialSpeed + Math.Pow((Bullet_InitialSpeed*Bullet_InitialSpeed + 2*Bullet_Acceleration*Vector3D.Distance(HitPoint,Me_Position)),0.5))/Bullet_Acceleration;       
			double HitTime_F = (-Bullet_InitialSpeed - Math.Pow((Bullet_InitialSpeed*Bullet_InitialSpeed + 2*Bullet_Acceleration*Vector3D.Distance(HitPoint,Me_Position)),0.5))/Bullet_Acceleration;       
			HitTime = (HitTime_Z > 0 ? (HitTime_F > 0 ? (HitTime_Z < HitTime_F ? HitTime_Z : HitTime_F) : HitTime_Z) : HitTime_F);       
			HitPoint = Target_Position + Velocity*HitTime + 0.5*Acceleration*HitTime*HitTime;       
		}       
	}       
	return HitPoint;       
}     
   
// ---------------- 航线内垂点计算函数 ------------   
// pos1 是航线外的点，pos2和pos3是航线的两个端点   
// 本函数返回pos1与航线的垂线，与该航线的交点座标   
Vector3D RouteVerticalPoint(Vector3D pos1, Vector3D pos2, Vector3D pos3)   
{   
	var Ratio_t = ((pos2.X - pos1.X)*(pos3.X - pos2.X) + (pos2.Y - pos1.Y)*(pos3.Y - pos2.Y) + (pos2.Z - pos1.Z)*(pos3.Z - pos2.Z))   
					/( Math.Pow(pos3.X-pos2.X,2) + Math.Pow(pos3.Y-pos2.Y,2) + Math.Pow(pos3.Z - pos2.Z,2));   
	double x = pos2.X - (pos3.X - pos2.X)*Ratio_t;   
	double y = pos2.Y - (pos3.Y - pos2.Y)*Ratio_t;   
	double z = pos2.Z - (pos3.Z - pos2.Z)*Ratio_t;   
	return new Vector3D(x,y,z);   
}   
   
// ============== 以下方块获取相关函数 ==================   
// 初始化母船所需方块函数   
IMyShipController MotherCockpit;   
IMyShipConnector MotherConnector;   
List<IMyTerminalBlock> DetachBlock;   
bool GetMotherShip()   
{   
	DetachBlock = new List<IMyTerminalBlock>();   
	for(int i = 0; i < DetachBlockNameTag.Length; i++){   
		if(DetachBlockNameTag[i] != "")   
		{   
			DetachBlock.Add(GridTerminalSystem.GetBlockWithName(DetachBlockNameTag[i]));   
		}   
		else{   
			List<IMyTerminalBlock> DetachBlock_Temp = new List<IMyTerminalBlock>();   
			switch (DetachBlockType[i])   
			{   
			case 0:   
				GridTerminalSystem.GetBlocksOfType<IMyShipMergeBlock> (DetachBlock_Temp);   
				break;   
			case 1:   
				GridTerminalSystem.GetBlocksOfType<IMyShipConnector> (DetachBlock_Temp);   
				break;   
			}   
			DetachBlock.Add(GetNearestTypeBlock(DetachBlock_Temp, Me));   
		}   
	}   
	IMyShipConnector connector = DetachBlock[0] as IMyShipConnector;   
	MotherConnector = connector.OtherConnector;   
	MotherCockpit = GridTerminalSystem.GetBlockWithName(MotherCockpitNameTag) as IMyShipController;   
	if(MotherCockpit == null){   
		Debug = "母船: " + MotherCockpitNameTag + " 未找到.";   
		return false;   
	}   
	Debug = "母船方块初始化完成";   
	Echo(DetachBlock[0].ToString());   
	Echo(MotherConnector.ToString());   
	return true;   
}   
   
// 初始化方块的函数   
IMyShipController Cockpit;   
IMyRadioAntenna Antenna;   
List<IMyTerminalBlock> Gyroscopes;   
List<IMyTerminalBlock> Thrusts;   
string[] ThrustField = null;   
string[] gyroYawField = null;   
string[] gyroPitchField = null;   
string[] gyroRollField = null;   
float[] gyroYawFactor = null;   
float[] gyroPitchFactor = null;   
float[] gyroRollFactor = null;   
const float GYRO_FACTOR = 1f;   
bool GetBlocks()   
{   
	if(CockpitNameTag != ""){   
		Cockpit = GridTerminalSystem.GetBlockWithName(CockpitNameTag) as IMyShipController;   
	}   
	else{   
		List<IMyTerminalBlock> Cockpit_Temp = new List<IMyTerminalBlock>();   
		GridTerminalSystem.GetBlocksOfType<IMyShipController> (Cockpit_Temp);   
		if(Cockpit_Temp.Count > 0)   
		{   
			Cockpit = Cockpit_Temp[0] as IMyShipController;   
		}   
	}   
	if(Cockpit == null){   
		Debug = CockpitNameTag + "  Null.";   
		return false;   
	}   
	   
	List<IMyRadioAntenna> Ant_temp = new List<IMyRadioAntenna>();   
	if(AntennaCustomTag != ""){   
		GridTerminalSystem.GetBlocksOfType(Thrusts, b => b.CustomData == AntennaCustomTag);   
	}   
	else{   
		GridTerminalSystem.GetBlocksOfType<IMyRadioAntenna> (Ant_temp);   
	}   
	if(Ant_temp.Count > 0){   
		Antenna = Ant_temp[0];   
	}   
   
	Thrusts = new List<IMyTerminalBlock>();   
	if(ThrustsNameTag != "")   
	{   
		GridTerminalSystem.SearchBlocksOfName(ThrustsNameTag, Thrusts);   
	}   
	else   
	{   
		GridTerminalSystem.GetBlocksOfType<IMyThrust> (Thrusts);   
	}   
	if(Thrusts.Count < 1)   
	{   
		Debug = "没有任何推进器";   
		return false;   
	}   
	else   
	{   
		ThrustField = new string[Thrusts.Count];   
		for(int i = 0; i < Thrusts.Count; i ++)   
		{   
			Base6Directions.Direction CockpitForward = Thrusts[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Forward);   
			Base6Directions.Direction CockpitUp = Thrusts[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Up);   
			Base6Directions.Direction CockpitLeft = Thrusts[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Left);   
			switch (CockpitForward)   
			{ case Base6Directions.Direction.Forward: ThrustField[i] = "Forward"; break; case Base6Directions.Direction.Backward: ThrustField[i] = "Backward"; break; }   
			switch (CockpitUp)   
			{ case Base6Directions.Direction.Forward: ThrustField[i] = "Up"; break; case Base6Directions.Direction.Backward: ThrustField[i] = "Down"; break; }   
			switch (CockpitLeft)   
			{ case Base6Directions.Direction.Forward: ThrustField[i] = "Left"; break; case Base6Directions.Direction.Backward: ThrustField[i] = "Right"; break; }   
		}   
	}   
   
	Gyroscopes = new List<IMyTerminalBlock>();   
	if(GyroscopesNameTag != "")   
	{   
		GridTerminalSystem.SearchBlocksOfName(GyroscopesNameTag, Gyroscopes);   
	}   
	else   
	{   
		GridTerminalSystem.GetBlocksOfType<IMyGyro> (Gyroscopes);   
	}   
	if(Gyroscopes.Count < 1)   
	{   
		Debug = "没有任何陀螺仪";   
		return false;   
	}   
   
	if(Gyroscopes.Count > 0)   
	{   
		gyroYawField = new string[Gyroscopes.Count]; gyroPitchField = new string[Gyroscopes.Count]; gyroYawFactor = new float[Gyroscopes.Count];   
		gyroPitchFactor = new float[Gyroscopes.Count]; gyroRollField = new string[Gyroscopes.Count]; gyroRollFactor = new float[Gyroscopes.Count];   
		for (int i = 0; i < Gyroscopes.Count; i++)   
		{   
			Base6Directions.Direction gyroUp = Gyroscopes[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Up);   
			Base6Directions.Direction gyroLeft = Gyroscopes[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Left);   
			Base6Directions.Direction gyroForward = Gyroscopes[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Forward);   
			switch (gyroUp)   
			{ case Base6Directions.Direction.Up: gyroYawField[i] = "Yaw"; gyroYawFactor[i] = GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Down: gyroYawField[i] = "Yaw"; gyroYawFactor[i] = -GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Left: gyroYawField[i] = "Pitch"; gyroYawFactor[i] = GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Right: gyroYawField[i] = "Pitch"; gyroYawFactor[i] = -GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Forward: gyroYawField[i] = "Roll"; gyroYawFactor[i] = -GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Backward: gyroYawField[i] = "Roll"; gyroYawFactor[i] = GYRO_FACTOR; break;   
			}   
			switch (gyroLeft)   
			{ case Base6Directions.Direction.Up: gyroPitchField[i] = "Yaw"; gyroPitchFactor[i] = GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Down: gyroPitchField[i] = "Yaw"; gyroPitchFactor[i] = -GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Left: gyroPitchField[i] = "Pitch"; gyroPitchFactor[i] = GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Right: gyroPitchField[i] = "Pitch"; gyroPitchFactor[i] = -GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Forward: gyroPitchField[i] = "Roll"; gyroPitchFactor[i] = -GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Backward: gyroPitchField[i] = "Roll"; gyroPitchFactor[i] = GYRO_FACTOR; break;   
			}   
   
			switch (gyroForward)   
			{ case Base6Directions.Direction.Up: gyroRollField[i] = "Yaw"; gyroRollFactor[i] = GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Down: gyroRollField[i] = "Yaw"; gyroRollFactor[i] = -GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Left: gyroRollField[i] = "Pitch"; gyroRollFactor[i] = GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Right: gyroRollField[i] = "Pitch"; gyroRollFactor[i] = -GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Forward: gyroRollField[i] = "Roll"; gyroRollFactor[i] = -GYRO_FACTOR; break;   
			  case Base6Directions.Direction.Backward: gyroRollField[i] = "Roll"; gyroRollFactor[i] = GYRO_FACTOR; break;   
			}   
			Gyroscopes[i].ApplyAction("OnOff_On");   
			SetGyroOverride(false);   
		}   
	}   
	Debug = "无人机方块初始化完成";   
	return true;   
}   
   
IMyTerminalBlock GetNearestTypeBlock(List<IMyTerminalBlock> Input_BlockList, IMyTerminalBlock SingleBlock)   
{   
	IMyTerminalBlock closestBlock = null;   
	double currDist = 0;   
	double closestDist = Double.MaxValue;   
	for (int i = 0; i < Input_BlockList.Count; i++)   
	{   
		currDist = (Input_BlockList[i].GetPosition() - SingleBlock.GetPosition()).Length();   
		if (currDist < closestDist)   
		{   
			closestDist = currDist;   
			closestBlock = Input_BlockList[i];   
		}   
	}   
	return closestBlock;   
}   
   
void TurnBlocksOn(List<IMyTerminalBlock> blocks, bool onoff)   
{   
	string act = (onoff ? "OnOff_On" : "OnOff_Off");   
	for(int i = 0; i < blocks.Count; i ++)   
	{   
		blocks[i].ApplyAction(act);   
	}   
}   
   
void SetThrustOverride(string Direction, double Value)   
{   
	if(Value < 0 || Value > 100){return;}
	if(Direction == "All")
	{for(int i = 0; i < Thrusts.Count; i ++){IMyThrust thrust = Thrusts[i] as IMyThrust; thrust.ThrustOverridePercentage = (float)Value;}}
	else
	{for(int i = 0; i < Thrusts.Count; i ++){if(ThrustField[i]==Direction){IMyThrust thrust = Thrusts[i] as IMyThrust; thrust.ThrustOverridePercentage = (float)Value;}}}   
}   
   
void SetGyroOverride(bool bOverride)   
{   
	for (int i = 0; i < Gyroscopes.Count; i++)   
	{if (((IMyGyro)Gyroscopes[i]).GyroOverride != bOverride){Gyroscopes[i].ApplyAction("Override");}}   
}   
   
void SetGyroYaw(double yawRate)   
{   
	for (int i = 0; i < Gyroscopes.Count; i++)   
	{Gyroscopes[i].SetValue(gyroYawField[i], (float)yawRate * gyroYawFactor[i]);}   
}   
   
void SetGyroPitch(double pitchRate)   
{   
	for (int i = 0; i < Gyroscopes.Count; i++)   
	{Gyroscopes[i].SetValue(gyroPitchField[i], (float)pitchRate * gyroPitchFactor[i]);}   
}   
   
void SetGyroRoll(double rollRate)   
{   
	for (int i = 0; i < Gyroscopes.Count; i++)   
	{Gyroscopes[i].SetValue(gyroRollField[i], (float)rollRate * gyroRollFactor[i]);}   
}
